COMMENT ⊗   VALID 00014 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00003 00002	BEGIN "LS"
C00005 00003	SUBR ROTATE(REFERENCE REAL X,Y REAL Q)
C00006 00004	SUBR MKCAMR		α COSINES BETWEEN CAMERA RAYS
C00008 00005	SUBR MKFORK		α INITIAL DIRECTION COSINES OF CAMERA RAYS
C00010 00006	SUBR SETFORK (REAL W1,W2,W3)
C00012 00007	SUBR SKEW (REAL MX,MY,MZ,NX,NY,NZ)
C00013 00008	SUBR MKFORCE		α FIND CLOSEST POINTS OF CORRESPONDING SKEW RAYS
C00015 00009	SUBR TORQUE (REAL W1,W2,W3)
C00017 00010	REAL SUBR QQ(REAL W1,W2,W3)
C00022 00011	SUBR TORKUP
C00024 00012	SUBR CLIMB
C00026 00013	PROCEDURE SAMPLER
C00027 00014	BEGIN "MAIN"		α MAIN EXECUTION
C00028 ENDMK
C⊗;
BEGIN "LS"
	REQUIRE "ABBREV[SYS,BGB]" SOURCE_FILE;

α SOLUTION TO TEST CASE;
	REAL C1X,C1Y,C1Z;
	REAL C2X,C2Y,C2Z;
	REAL WW1,WW2,WW3;

α TWO CAMERA FORKS;
	REAL SS1,SS2,RR1,RR2;			α CAMERA CIRCLE CENTERS & RADII;
	REAL α1,α2,MS12,NS12;			α ANGLE BETWEEN FIRST TWO RAYS;
	REAL MC12,MC13,MC14,MC15,MC23,MC24,MC25,MC34,MC35,MC45;
	REAL NC12,NC13,NC14,NC15,NC23,NC24,NC25,NC34,NC35,NC45;
	REAL MSIGN3,MSIGN4,MSIGN5,NSIGN3,NSIGN4,NSIGN5;

α POSITION DIRECTION COSINES ACCORDING TO W1,W2,W3;
	REAL M3X,M3Y,M3Z;	REAL M4X,M4Y,M4Z;	REAL M5X,M5Y,M5Z;
	REAL N3X,N3Y,N3Z;	REAL N4X,N4Y,N4Z;	REAL N5X,N5Y,N5Z;
	REAL U3X,U3Y,U3Z;	REAL U4X,U4Y,U4Z;	REAL U5X,U5Y,U5Z;
	REAL V3X,V3Y,V3Z;	REAL V4X,V4Y,V4Z;	REAL V5X,V5Y,V5Z;
	REAL UX,UY,UZ;		REAL VX,VY,VZ;

α INITIAL CAMERA RAY DIRECTION COSINES;
	REAL M30X,M30Y,M30Z;
	REAL M40X,M40Y,M40Z;
	REAL M50X,M50Y,M50Z;

	REAL N30X,N30Y,N30Z;
	REAL N40X,N40Y,N40Z;
	REAL N50X,N50Y,N50Z;

α PARAMETERS OF CLOSEST APPROACH BETWEEN CORRESPONDING SKEW RAYS;
	REAL P,Q;
	REAL F3X,F3Y,F3Z,D3;
	REAL F4X,F4Y,F4Z,D4;
	REAL F5X,F5Y,F5Z,D5;

	REAL DW0,DW1,DW2,DW3;
SUBR ROTATE(REFERENCE REAL X,Y; REAL Q);
BEGIN "ROTATE"
	REAL C,S,T;
	C ← COS(Q); S ← SIN(Q);
	T ← C*X - S*Y;
	Y ← C*Y + S*X;
	X ← T;
END "ROTATE";
SUBR MKCAMR;		α COSINES BETWEEN CAMERA RAYS;
BEGIN "MKCAMR"

	MC12 ← .8630286; NC12 ←.6801704;	α TEST COSINES;
	MC13 ← .9280318; NC13 ←.8939879;
	MC14 ← .9504978; NC14 ←.8864540;
	MC15 ← .8634228; NC15 ←.8354626;

	MC23 ← .9674100; NC23 ←.9180645;
	MC24 ← .9387822; NC24 ←.8159213;
	MC25 ← .9958401; NC25 ←.9708753;

	MC34 ← .9240093; NC34 ←.8619705;
	MC35 ← .9805967; NC35 ←.9764107;
	MC45 ← .9185642; NC45 ←.9068553;

	MSIGN3 ← -1;	MSIGN4 ← +1;	MSIGN5 ← -1;	α TEST SIGNS;
	NSIGN3 ← -1;	NSIGN4 ← +1;	NSIGN5 ← +1;

	α1 ← ACOS(MC12); MS12 ← SIN(α1);
	α2 ← ACOS(NC12); NS12 ← SIN(α2);

	SS1 ← MC12/MS12; RR1 ← 1/MS12;
	SS2 ← NC12/NS12; RR2 ← 1/NS12;
	
α TEST SOLUTION;
	C1X ← 3.628192;	C1Y ← -.4830575; C1Z ← 0;
	C2X ← 2.211233; C2Y ←  .2988597; C2Z ←.4614799;

	WW1 ← ATAN2(C1Y,C1X-SS1);
	WW2 ← ASIN(C2Y/RR2);
	WW3 ← ATAN2(-C2Z,C2X);
END "MKCAMR";
SUBR MKFORK;		α INITIAL DIRECTION COSINES OF CAMERA RAYS;
BEGIN "MKFORK"
	M30X ← -MC13;
	M30Y ←-(MC23-MC12*MC13)/MS12;
	M30Z ← MSIGN3*SQRT(1-M30X↑2-M30Y↑2);

	M40X ← -MC14;
	M40Y ←-(MC24-MC12*MC14)/MS12;
	M40Z ← MSIGN4*SQRT(1-M40X↑2-M40Y↑2);

	M50X ← -MC15;
	M50Y ←-(MC25-MC12*MC15)/MS12;
	M50Z ← MSIGN5*SQRT(1-M50X↑2-M50Y↑2);

	N30X ← -NC13;
	N30Y ←-(NC23-NC12*NC13)/NS12;
	N30Z ← NSIGN3*SQRT(1-N30X↑2-N30Y↑2);

	N40X ← -NC14;
	N40Y ←-(NC24-NC12*NC14)/NS12;
	N40Z ← NSIGN4*SQRT(1-N40X↑2-N40Y↑2);

	N50X ← -NC15;
	N50Y ←-(NC25-NC12*NC15)/NS12;
	N50Z ← NSIGN5*SQRT(1-N50X↑2-N50Y↑2);
END "MKFORK";
SUBR SETFORK (REAL W1,W2,W3);
BEGIN "SETFORK"
	REAL OM1,OM2;

α COPY THE INITIAL FORK VECTORS;
	M3X ← M30X;	M3Y ← M30Y;	M3Z ← M30Z;
	M4X ← M40X;	M4Y ← M40Y;	M4Z ← M40Z;
	M5X ← M50X;	M5Y ← M50Y;	M5Z ← M50Z;

	N3X ← N30X;	N3Y ← N30Y;	N3Z ← N30Z;
	N4X ← N40X;	N4Y ← N40Y;	N4Z ← N40Z;
	N5X ← N50X;	N5Y ← N50Y;	N5Z ← N50Z;

α FORK ORIENTATION ANGLES;
	OM1 ← ATAN2(1-RR1*SIN(W1),SS1+RR1*COS(W1));
	OM2 ← ATAN2(1-RR2*SIN(W2),SS2+RR2*COS(W2));
	ROTATE(M3X,M3Y,-OM1);	ROTATE(M4X,M4Y,-OM1);	ROTATE(M5X,M5Y,-OM1);
	ROTATE(N3X,N3Y,-OM2);	ROTATE(N4X,N4Y,-OM2);	ROTATE(N5X,N5Y,-OM2);
	ROTATE(N3Z,N3X,W3);	ROTATE(N4Z,N4X,W3);	ROTATE(N5Z,N5X,W3);

α CAMERA LOCII;
	UX ←  SS1 + RR1*COS(W1);
	UY ←        RR1*SIN(W1);
	UZ ←  0;

	VX ← COS(W3)*(SS2 + RR2*COS(W2));
	VY ←                RR2*SIN(W2) ;
	VZ ←-SIN(W3)*(SS2 + RR2*COS(W2));
END "SETFORK";
SUBR SKEW (REAL MX,MY,MZ,NX,NY,NZ);
BEGIN "SKEW"
	REAL MN;
	MN ← MX*NX + MY*NY + MZ*NZ;

	P ←	((UX-VX)*(NX*MN-MX)
		+(UY-VY)*(NY*MN-MY)
		+(UZ-VZ)*(NZ*MN-MZ)) / (1-MN↑2);

	Q ←	((UX-VX)*(NX-MN*MX)
		+(UY-VY)*(NY-MN*MY)
		+(UZ-VZ)*(NZ-MN*MZ)) / (1-MN↑2);
END "SKEW";
SUBR MKFORCE;		α FIND CLOSEST POINTS OF CORRESPONDING SKEW RAYS;
BEGIN "MKFORCE"
	SKEW(M3X,M3Y,M3Z,N3X,N3Y,N3Z);
	U3X ← UX + P*M3X;	V3X ← VX + Q*N3X;
	U3Y ← UY + P*M3Y;	V3Y ← VY + Q*N3Y;
	U3Z ← UZ + P*M3Z;	V3Z ← VZ + Q*N3Z;

	SKEW(M4X,M4Y,M4Z,N4X,N4Y,N4Z);
	U4X ← UX + P*M4X;	V4X ← VX + Q*N4X;
	U4Y ← UY + P*M4Y;	V4Y ← VY + Q*N4Y;
	U4Z ← UZ + P*M4Z;	V4Z ← VZ + Q*N4Z;

	SKEW(M5X,M5Y,M5Z,N5X,N5Y,N5Z);
	U5X ← UX + P*M5X;	V5X ← VX + Q*N5X;
	U5Y ← UY + P*M5Y;	V5Y ← VY + Q*N5Y;
	U5Z ← UZ + P*M5Z;	V5Z ← VZ + Q*N5Z;

α FORCE VECTORS;
	F3X ← U3X-V3X;	F3Y ← U3Y-V3Y;	F3Z ← U3Z-V3Z;
	F4X ← U4X-V4X;	F4Y ← U4Y-V4Y;	F4Z ← U4Z-V4Z;
	F5X ← U5X-V5X;	F5Y ← U5Y-V5Y;	F5Z ← U5Z-V5Z;

α SEPARATION DISTANCE;
	D3 ← SQRT(F3X↑2+F3Y↑2+F3Z↑2);
	D4 ← SQRT(F4X↑2+F4Y↑2+F4Z↑2);
	D5 ← SQRT(F5X↑2+F5Y↑2+F5Z↑2);
END "MKFORCE";
SUBR TORQUE (REAL W1,W2,W3);
BEGIN "TORQUE"
	DW1 ← -F3X*U3Y + F3Y*(U3X-SS1)
	      -F4X*U4Y + F4Y*(U4X-SS1)
	      -F5X*U5Y + F5Y*(U5X-SS1);

	DW2 ← -SIN(W3)*
			(+F3Y*(V3Z+SS2*SIN(W3)) - F3Z*V3Y
			 +F4Y*(V4Z+SS2*SIN(W3)) - F4Z*V4Y
			 +F5Y*(V5Z+SS2*SIN(W3)) - F5Z*V5Y)
	      +COS(W3)*
			(-F3X*(V3Z+SS2*SIN(W3)) + F3Z*(V3X-SS2*COS(W3))
			 -F4X*(V4Z+SS2*SIN(W3)) + F4Z*(V4X-SS2*COS(W3))
			 -F5X*(V5Z+SS2*SIN(W3)) + F5Z*(V5X-SS2*COS(W3)) );

	DW3 ← + F3X*V3Z - F3Z*V3X
	      + F4X*V4Z - F4Z*V4X
	      + F5X*V5Z - F5Z*V5X;

	DW0 ← SQRT (DW1↑2 + DW2↑2 + DW3↑2);
	DW1 ← DW1/DW0;
	DW2 ← DW2/DW0;
	DW3 ← DW3/DW0;
END "TORQUE";
REAL SUBR QQ(REAL W1,W2,W3);
BEGIN "QQ"
	SETFORK(W1,W2,W3);
	MKFORCE;
	RETURN(1/(1+D3+D4+D5));
END "QQ";
SUBR TORKUP;
BEGIN "TORKUP"
	REAL W1,W2,W3;	STRING STR;ITG CHR;REAL Q0,Q1,Q2,Q3,Q4,Q00;
	REAL P0,P1,P2,P3,P4;REAL D1,D2;ITG I,J;
	REAL K;
	ITG IMAX;

OUTSTR("INITIAL GUESS & K ? ");
	STR←INCHWL;
α	W1←REALSCAN(STR,CHR)*π/180;
α	W2←REALSCAN(STR,CHR)*π/180;
α	W3←REALSCAN(STR,CHR)*π/180;
	K←REALSCAN(STR,CHR);
	IMAX←REALSCAN(STR,CHR);
	W1←W2←W3←0;
	Q0 ← QQ(W1,W2,W3);

FOR I←0 THRU IMAX DO
BEGIN
	Q0 ← QQ(W1,W2,W3);
TORQUE(W1,W2,W3);
OUTSTR(CVS(I)&CVG(Q0)&CVG(K*DW1*180/π)&CVG(K*DW2*180/π)&CVG(K*DW3*180/π)&↓);
	W1 ← W1-K*DW1;	W2 ← W2-K*DW2;	W3 ← W3-K*DW3;
END;
OUTSTR("W ANGLE'S:	"&CVG(180*W1/π)&" "&CVG(180*W2/π)&" "&CVG(180*W3/π)&↓);
END "TORKUP";
SUBR CLIMB;
BEGIN "CLIMB"
	REAL W1,W2,W3;	STRING STR;ITG CHR;REAL Q0,Q1,Q2,Q3,Q4,Q00;
	REAL P0,P1,P2,P3,P4;REAL D1,D2;ITG I,J;

OUTSTR("INITIAL GUESS ? ");
	STR←INCHWL;
	W1←REALSCAN(STR,CHR)*π/180;
	W2←REALSCAN(STR,CHR)*π/180;
	W3←REALSCAN(STR,CHR)*π/180;
	
	D1 ← 1@-5;
	D2 ← 1@3;
	Q0 ← QQ(W1,W2,W3);

FOR I←0 THRU 300 DO
BEGIN
	OUTSTR(CVS(I)&" "&CVG(Q0)&CVG(D2)&↓);
	Q1 ← QQ(W1+D1,W2,W3)-Q0;
	Q2 ← QQ(W1,W2+D1,W3)-Q0;
	Q3 ← QQ(W1,W2,W3+D1)-Q0;

α SHORTEN THE STEP SIZE;
	WHILE QQ(W1+D2*Q1,W2+D2*Q2,W3+D2*Q3)<Q0 DO
	IF D2=0.1 THEN DONE ELSE D2 ← (D2*0.80 MAX 0.1);

	IF D2=0.1 THEN 
	IF D1=1@-5 THEN ⊂ D1←1@-6; D2←1.0;⊃ ELSE
	IF D1=1@-6 THEN ⊂ D1←1@-7; D2←1.0;⊃ ELSE DONE;

α LENGTHEN THE STEP SIZE;
	IF D2<300 ∧ D2>0.1 THEN
	WHILE Q0<(Q00←QQ(W1+1.2*D2*Q1,W2+1.2*D2*Q2,W3+1.2*D2*Q3)) DO 
	⊂ Q0←Q00;D2←D2*1.2;⊃;
	Q0←QQ(W1←W1+D2*Q1,W2←W2+D2*Q2,W3←W3+D2*Q3);
END;
	OUTSTR(CVS(I)&" "&CVG(Q0)&CVG(D2)&↓);
OUTSTR("W ANGLE'S:	"&CVG(180*W1/π)&" "&CVG(180*W2/π)&" "&CVG(180*W3/π)&↓);
END "CLIMB";
PROCEDURE SAMPLER;
BEGIN "SAMPLER"
	SAFE REAL ARRAY FN[-15:+15,-15:+15,-15:+15];
	ITG I,J,K,IM,JM,KM;
	REAL W1,W2,W3,QMAX,Q;

	QMAX←0;
	FOR K←-30 STEP 2 UNTIL 30 DO ⊂ W3 ← K*π/180;
	FOR J←-30 STEP 2 UNTIL 30 DO ⊂ W2 ← J*π/180;
	FOR I←-30 STEP 2 UNTIL 30 DO ⊂ W1 ← I*π/180;
	Q←QQ(W1,W2,W3);FN[I/2,J/2,K/2]←Q;
	IF Q>QMAX THEN ⊂ QMAX←Q;IM←I;JM←J;KM←K;
	OUTSTR(CVG(QMAX)&" "&CVS(I)&" "&CVS(J)&" "&CVS(K)&↓);⊃;
	⊃;⊃;⊃;
OPEN(1,"DSK",8,0,3,0,0,0);
ENTER(1,"FN3D",0);
ARRYOUT(1,FN[-15,-15,-15],31↑3);
RELEASE(1);
OUTSTR("DONE");INCHRW;
END "SAMPLER";
BEGIN "MAIN"		α MAIN EXECUTION;
	REAL Q0,W1,W2,W3;
	ITG I,J,K;

	MKCAMR;
	MKFORK;
	Q0 ← QQ(WW1,WW2,WW3);
	OUTSTR("TEST SOLUTION: "&CVG(WW1)&CVG(WW2)&CVG(WW3)&↓);
	OUTSTR(" QQ = "&CVG(Q0)&↓);
	OUTSTR(" D3 = "&CVG(D3)&↓);
	OUTSTR(" D4 = "&CVG(D4)&↓);
	OUTSTR(" D5 = "&CVG(D5)&↓);

	WHILE TRUE DO SAMPLER;

END "MAIN";

END "LS";